#include "include/PID.h"

int main()
{
    PID pid_dis = {0}; 
    PID pid_ang = {0}; 
    PID_Init(&pid_dis, 0.5, 0.2, 0.1, 10, 5);
    PID_Init(&pid_ang, 0.5, 0.2, 0.1, 10, 1);
    
    float set_dis = 50.0f; //目标值
    float set_ang = 180.0f; //目标值

    float *now_dis = (float *)malloc(sizeof(float));
    *now_dis = 0.0f;
    float *now_ang = (float *)malloc(sizeof(float));
    *now_ang = 0.0f;

    while(fabs(set_dis - *now_dis) > 2)
    {
        PID_Calc(&pid_dis, set_dis, *now_dis); 
        PID_Calc(&pid_dis, set_ang, *now_ang); 
    }
    free(now_dis);
    free(now_ang);
    
}